(*DONT_TOUCH = "TRUE"*)
module pid_cmd (
    input clk,                // 50MHz时钟
    input rst_n,              // 低电平复位
    input pid_en,             // PID使能信号
    input device_0_en,        // 电机总使能
    input [31:0] count,       // 实际颗粒计数
    input [31:0] target_count,// 目标颗粒数
    output device_0_pwm,      // PWM输出信号
    output stop_pluse    // 达标停止信号
);

// 配置参数（整数放大100倍后的值）
parameter KP_SCALED = 25;     // 原KP=0.25 → 0.25*100=25
parameter KI_SCALED = 0;      // 原KI=0.0625 → 0.0625*100=6
parameter KD_SCALED = 100;    // 原KD=1.0 → 1.0*100=100

// 系统参数
parameter SAMPLE_RATE = 250;  // 采样周期250 (200kHz)
parameter MAX_DUTY = 255;     // 最大占空比
parameter INTEGRAL_LIMIT = 3000;  // 积分限幅（±3000）
parameter SCALE_DIV = 100;    // 最终输出缩小倍数

// FIFO参数
parameter FIFO_DEPTH = 100;

// 控制信号声明
reg [31:0] sample_counter;    // 采样分频
reg signed [31:0] error;      // 当前误差
reg signed [31:0] last_error; // 上次误差
reg signed [31:0] integral;   // 积分累加值
reg signed [31:0] p_term, i_term, d_term; // P/I/D项
reg [7:0] duty_cycle;         // 最终占空比
reg stop_signal;

// FIFO寄存器：存储历史count值
reg [31:0] count_fifo [0:FIFO_DEPTH-1];
integer i;

// PWM模块实例化
pwm_generator u_pwm (
    .clk(clk),
    .rst_n(rst_n),
    .enable(device_0_en),
    .duty_cycle(duty_cycle),
    .pwm_out(device_0_pwm)
);

// 采样时钟生成
always @(posedge clk or negedge rst_n) begin
    if (!rst_n) 
        sample_counter <= 0;
    else 
        sample_counter <= (sample_counter >= SAMPLE_RATE) ? 0 : sample_counter + 1;
end

// 核心PID计算
always @(posedge clk or negedge rst_n) begin
    if (!rst_n) begin
        error <= 0;
        last_error <= 0;
        integral <= 0;
        p_term <= 0;
        i_term <= 0;
        d_term <= 0;
        duty_cycle <= 0;
        stop_signal <= 0;
        // FIFO初始化
        for (i = 0; i < FIFO_DEPTH; i = i + 1) begin
            count_fifo[i] <= 0;
        end
    end else if (!pid_en && device_0_en) begin
        duty_cycle <= 8'd255; // 非PID模式全速
    end
    else if (device_0_en && pid_en) begin //防止未进入pid模式也到达目标颗粒后也发送关断指令
        if (count >= target_count) begin
            // 达标停止
            stop_signal <= 1'b1;
            duty_cycle <= 0;
            integral <= 0;
        end else if (sample_counter == 0 && pid_en) begin
            // 误差计算
            //error <= target_count - count;
            // 用FIFO队头（较早的count值）来计算误差
            error <= target_count - count_fifo[0];
            // FIFO更新：队头数据出队，新的count入队尾
            for (i = 0; i < FIFO_DEPTH - 1; i = i + 1) begin
                count_fifo[i] <= count_fifo[i+1];
            end
            count_fifo[FIFO_DEPTH - 1] <= count;
            //================ P项计算 ================
            p_term <= error * KP_SCALED; // P项放大100倍
            
            //================ I项计算 ================
            if (integral > INTEGRAL_LIMIT && error > 0) begin
                integral <= integral; // 正饱和保持
            end else if (integral < -INTEGRAL_LIMIT && error < 0) begin
                integral <= integral; // 负饱和保持
            end else begin
                integral <= integral + error; // 积分累加
            end
            i_term <= integral * KI_SCALED;
            
            //================ D项计算 ================
            d_term <= (error - last_error) * KD_SCALED;
            last_error <= error;
            
            //================ PID合成 ================
            // 总输出 = (P + I + D)/100 （缩小到原值）
            duty_cycle <= adjust_output(p_term + i_term + d_term);
            
            stop_signal <= 1'b0;
        end
    end else begin
        duty_cycle <= 0;
        stop_signal <= 1'b0;
    end
end

function [7:0] adjust_output(input signed [31:0] pid_total);
    reg signed [31:0] scaled;
    reg [15:0] product;
    reg [15:0] sum_with_rounding;
    //reg [7:0] divided;
    reg [7:0] mapped;
    begin
        // 缩小100倍并限幅到0-255
        scaled = pid_total / SCALE_DIV; // SCALE_DIV = 100
        if (scaled > MAX_DUTY)          // MAX_DUTY = 255
            scaled = MAX_DUTY;
        else if (scaled < 0)
            scaled = 0;

        // 修改为160-223范围：
        product = scaled <<5; // 32+2=34
        mapped = product >>7;                  // 34/128
        adjust_output = mapped + 170;

        // // 将0-255线性映射到128-255
        // product = scaled << 7;             // 计算x*127
        // sum_with_rounding = product + 127;  // 加127以四舍五入
        // divided = sum_with_rounding >> 7;  // 除以255
        // adjust_output = divided + 8'd128;   // 加上128得到最终值
    end
endfunction

// // 输出调整函数：缩小100倍并限幅
// function [7:0] adjust_output(input signed [31:0] pid_total);
//     reg signed [31:0] scaled;
//     begin
//         scaled = pid_total / SCALE_DIV; // 整数除法缩小100倍
//         if (scaled > MAX_DUTY)
//             adjust_output = MAX_DUTY;
//         else if (scaled < 0)
//             adjust_output = 0;
//         else
//             adjust_output = scaled[7:0];
//     end
// endfunction

single_pluse u_single_pluse(
    .clk(clk),
    .rst_n(rst_n),
    .signal_in(stop_signal),
    .pluse_out(stop_pluse)
);

endmodule
